#include <ros/ros.h>
#include <math.h>
#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>

#define PI 3.1415926

using namespace std;

double t = 0, tstart = 0, T[1000] = {0};
int command = 0, flag = 0;
int track_point_times_ = 1;
double pos_init[3], pos_traget[3];
double obstaclepos[3], posob_init[3], posob_target[3];
double r_circle, pi_circle;
double tello_center[3];
double cen_ob; // 飞机中心离障碍物的距离
int i = 0, j = 0;
int k = -0.6;

typedef struct
{
  double t;
  double pos[3];
  string type;
  double omega;
} WAYPOINT;
std::vector<WAYPOINT> traj_points;

void cb_readcenter(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  geometry_msgs::PoseStamped pos;
  pos = *msg;

  tello_center[0] = pos.pose.position.x;
  tello_center[1] = pos.pose.position.y;
  cout << "收到中心位置" << tello_center[0] << " " << tello_center[1] << endl;
}

void cb_command(const std_msgs::Int32::ConstPtr &msg)
{
  std_msgs::Int32 cmd_data;
  cmd_data = *msg;
  command = cmd_data.data;
  cout << "receive command: " << command << endl;
  if (command == 2 && flag == 0)
  {
    tstart = ros::Time::now().toSec();
    flag = 1;
  }
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "pos_controller");
  ros::NodeHandle nh("");
  // 定义发布与订阅
  ros::Subscriber cmd_sub1 = nh.subscribe("/command", 10, cb_command);
  ros::Publisher poscmd1 = nh.advertise<geometry_msgs::PoseStamped>("/pos_cmd", 10);
  ros::Publisher stopcmd = nh.advertise<std_msgs::Int32>("/command", 10);
  ros::Subscriber pos_sub1 = nh.subscribe("/tello_center", 10, cb_readcenter);
  ros::NodeHandle nh_private("~"); // 设置私有参数服务器
  XmlRpc::XmlRpcValue waypoints;
  nh_private.getParam("waypoints", waypoints);//用私人参数服务器根据关键词获取waypoints
  
  geometry_msgs::PoseStamped hover; // 期望的位置点
  // 录入位置点信息
  for (i = 0; i < waypoints.size(); ++i)
  {
    XmlRpc::XmlRpcValue point = waypoints[i];
    WAYPOINT traj_point;
    traj_point.t = point["t"];//有点类似python list的使用方法
    traj_point.pos[0] = point["pos"][0];
    traj_point.pos[1] = point["pos"][1];
    traj_point.pos[2] = point["pos"][2];
    string type = point["type"];
    traj_point.type = type;
    cout << traj_point.pos[0] << " " << traj_point.pos[1] << endl;
    if (traj_point.type == "circle_center")
    {
      traj_point.omega = point["omega"];
    }
    traj_points.emplace_back(traj_point);
    cout << "traj_points: " << traj_points[i].pos << endl;
    T[i] = traj_points[i].t;
  }
  ros::Duration(0.5).sleep(); // 用于确保pos_init可以读取到tello_center的数据
  ros::spinOnce();
  // 重置i的计数,并确定初始位置
  i = 0;
  pos_init[0] = tello_center[0];
  pos_init[1] = tello_center[1];
  ros::Rate loop_rate(50);
  while (ros::ok())
  {
    t = ros::Time::now().toSec() - tstart;
    cout << "t:" << t << endl;
    cout << "T[i]:" << T[i] << " ";
    if(command == 0)
    {
      hover.pose.position.x = traj_points[i].pos[0];
      hover.pose.position.y = traj_points[i].pos[1];
      // cout<<"编队初始化的位置为:"<<hover.pose.position.x<<" "<<hover.pose.position.y;
      poscmd1.publish(hover);
      pos_init[0] = tello_center[0];
      pos_init[1] = tello_center[1];
    }
    if (command == 2) // 开始形成编队
    {
      cout << "i = "<< i <<endl;
      if (i < traj_points.size())
      {
        if (i > 0)
          cout << "T[i-1]:" << T[i - 1] << endl;
        // cout << "i:" << i << endl;
        pos_traget[0] = traj_points[i].pos[0];
        pos_traget[1] = traj_points[i].pos[1];
        cout << "初始位置" << pos_init[0] << " " << pos_init[1] << endl;
        cout << "期望位置" << pos_traget[0] << " " << pos_traget[1] << endl;
        // cout << "位置指令" << hover.position_x << " " << hover.position_y << endl;
        // 根据开始fly_to_car的时间计算本次控制循环的期望位置：hover_x,hover_y,本质为逐步改变期望位置
        hover.pose.orientation.w = 0;
        hover.pose.orientation.z = 0;
        if (pos_init[0] == pos_traget[0] && pos_init[1] == pos_traget[1] && i>=3)
          hover.pose.orientation.w = 1;
        if (i == traj_points.size())
          hover.pose.orientation.z = 1;
        // 直线运行
        if (traj_points[i].type == "fixed")
        {
          hover.pose.position.x = pos_init[0] + (pos_traget[0] - pos_init[0]) * (t - T[i - 1]) / (T[i] - T[i - 1]);
          hover.pose.position.y = pos_init[1] + (pos_traget[1] - pos_init[1]) * (t - T[i - 1]) / (T[i] - T[i - 1]);
          hover.pose.orientation.x = T[i - 1];
          hover.pose.orientation.y = T[i];
        }
        // 弧线运行
        if (traj_points[i].type == "circle_center")
        {
          r_circle = sqrt(pow(pos_traget[0] - pos_init[0], 2) + pow(pos_traget[1] - pos_init[1], 2)); // 计算半径
          pi_circle = atan2(pos_traget[1] - pos_init[1], pos_traget[0] - pos_init[0]) + PI;
          hover.pose.position.x = pos_traget[0] + r_circle * cos(traj_points[i].omega * (t - T[i - 1]) + pi_circle);
          hover.pose.position.y = pos_traget[1] + r_circle * sin(traj_points[i].omega * (t - T[i - 1]) + pi_circle);
        }
        poscmd1.publish(hover);
        if (abs(t - T[i]) < 0.02) // 认为到达状态切换位置
        {
          pos_init[0] = pos_traget[0] ;
          pos_init[1] = pos_traget[1] ;
          i++;
          // ros::Duration(0.2).sleep();
        }
      }
      else
      {
        cout << "飞机完成任务，开始悬停" << endl;
        // hover.pose.position.x = tello_center[0];
        // hover.pose.position.y = tello_center[1];
        hover.pose.position.x = pos_traget[0];
        hover.pose.position.y = pos_traget[1];
        poscmd1.publish(hover);
        // std_msgs::Int32 command;
        // command.data = 3;
        // stopcmd.publish(command);
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}